/*
 * Copyright (c) 2021, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "board.h"
#include "ti/driverlib/dl_gpio.h"
#include "ti/driverlib/dl_timer.h"
#include "ti_msp_dl_config.h"
#include "oled.h"
#include "bsp_mpu6050.h"
#include "inv_mpu.h"
#include "myOLED.h"
#include "MOTOR.h"
#include "delay.h"
#include "Key.h"
#include "LED.h"
#include "FIndLine.h"
#include "PID.h"

//uint32_t period = 20000;
char txBuf[100];
uint8_t status;



// PID参数

double pwm1out = 0,pwm2out = 0;//左，右
#define MID 400

#define ANGLEOblique   -45      //斜线角度
#define ANGLEOblique2   205   //斜线角度
#define ANGLEStraight   168     //直线角度
#define ANGLEError      10       //偏差角度 

int lastLaw[5]          = {0,               -17,    0,  0,  0};
int angleOblique[5]     = {ANGLEOblique,    -56,    0,  0,  0};
int angleOblique2[5]    = {ANGLEOblique2,   191,      0,  0,  0};
int  angleStraight[5]   = {ANGLEStraight,   153,      0,  0,  0};

int cnt = 1;
int flag = 1;

float last_yaw;
volatile float pitch=0,roll=0,yaw=0;

int abs(int a);
void Init_MPU_DMP();
void MyOLED_ShowMode();

// int main(void)
// {
//     SYSCFG_DL_init();
// }


int main(void)
{
    SYSCFG_DL_init();

    //NVIC_EnableIRQ(GPIO_BUTTON_INT_IRQN);
    //NVIC_EnableIRQ(TIMER_1_INST_INT_IRQN);
    NVIC_ClearPendingIRQ(UART_0_INST_INT_IRQN);
    NVIC_EnableIRQ(UART_0_INST_INT_IRQN);
    OLED_Init();
    board_init();
	MPU6050_Init();
    NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN);
    

    while(1) 
    {
        Init_MPU_DMP(); 
        MyOLED_ShowMode();
        

        while(Key_GetEnter()==0)
        {
            if(Key_GetChoice()!=0)
            { 
                cnt++;
                if(cnt>4)   cnt = 1;
                MyOLED_ShowString(1, 7, (uint8_t*)" ");
                MyOLED_ShowString(2, 7, (uint8_t*)" ");
                MyOLED_ShowString(3, 7, (uint8_t*)" ");
                MyOLED_ShowString(4, 7, (uint8_t*)" ");
                MyOLED_ShowString(cnt, 7, (uint8_t*)"<");
            }
        }

        switch (cnt) {
            case 1:
                OLED_Clear();
                while(Find_Start())
                {
                    while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0);
                    double pid_output = calculate_pid(yaw,last_yaw);
                    pwm1out = MID+pid_output;
                    pwm2out = MID-pid_output;
                
                    if(pwm1out >500) pwm1out=500;//进行限幅
                    if(pwm1out <0) pwm1out =0;
                    if(pwm2out >500) pwm2out =500;
                    if(pwm2out<0) pwm2out =0;
                    motorPidSetSpeed(pwm1out,pwm2out);//给参数电机参数的
                    
                    MyOLED_ShowNum(1,1,pwm1out,3);
                    MyOLED_ShowNum(2,1,pwm2out,3);
                    if(yaw>=0)
                    {
                        MyOLED_ShowNum(3,1,yaw,3);
                        MyOLED_ShowChar(3, 5, '+');
                    }
                    else {
                        MyOLED_ShowNum(3,1,-yaw,3);
                        MyOLED_ShowChar(3, 5, '-');
                    }
                }
                LED_ON();
                DL_TimerG_startCounter(TIMER_0_INST);
                motorPidSetSpeed(0,0);
            break;
        
            case 2:
                //直行
                OLED_Clear();
                while(Find_Start())
                {
                    while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0);
                    double pid_output = calculate_pid(yaw,last_yaw);
                    pwm1out = MID+pid_output;
                    pwm2out = MID-pid_output;
                
                    if(pwm1out >500) pwm1out=500;//进行限幅
                    if(pwm1out <0) pwm1out =0;
                    if(pwm2out >500) pwm2out =500;
                    if(pwm2out<0) pwm2out =0;
                    motorPidSetSpeed(pwm1out,pwm2out);//给参数电机参数的
                    
                    MyOLED_ShowNum(1,1,pwm1out,3);
                    MyOLED_ShowNum(2,1,pwm2out,3);
                    if(yaw>=0)
                    {
                        MyOLED_ShowNum(3,1,yaw,3);
                        MyOLED_ShowChar(3, 5, '+');
                    }
                    else {
                        MyOLED_ShowNum(3,1,-yaw,3);
                        MyOLED_ShowChar(3, 5, '-');
                    }
                }
                DL_TimerG_startCounter(TIMER_0_INST);
                LED_ON();
                //曲线
                OLED_Clear();
                while(Find_End() || abs(yaw-ANGLEStraight)>ANGLEError)
                {
                    while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0);
                    if(yaw<0)   yaw+=360;   
                    scan();
                    Show_HW();
                    run();
                    MyOLED_ShowNum(1,1,
                        DL_TimerG_getCaptureCompareValue(PWM_1_INST, DL_TIMER_CC_0_INDEX),3);
                    MyOLED_ShowNum(2,1,
                        DL_TimerG_getCaptureCompareValue(PWM_1_INST, DL_TIMER_CC_1_INDEX),3); 
                }
                DL_TimerG_startCounter(TIMER_0_INST);
                LED_ON();

                //直行
                OLED_Clear();
                while(Find_Start())
                {
                    while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0);
                    if(yaw<0)   yaw+=360;
                    double pid_output = calculate_pid(yaw,ANGLEStraight);
                    pwm1out = MID+pid_output;
                    pwm2out = MID-pid_output;
                
                    if(pwm1out >500) pwm1out=500;//进行限幅
                    if(pwm1out <0) pwm1out =0;
                    if(pwm2out >500) pwm2out =500;
                    if(pwm2out<0) pwm2out =0;
                    motorPidSetSpeed(pwm1out,pwm2out);//给参数电机参数的
                    
                    MyOLED_ShowNum(1,1,pwm1out,3);
                    MyOLED_ShowNum(2,1,pwm2out,3);
                    if(yaw>=0)
                    {
                        MyOLED_ShowNum(3,1,yaw,3);
                        MyOLED_ShowChar(3, 5, '+');
                    }
                    else {
                        MyOLED_ShowNum(3,1,-yaw,3);
                        MyOLED_ShowChar(3, 5, '-');
                    }
                }
                DL_TimerG_startCounter(TIMER_0_INST);
                LED_ON();


                //曲线
                OLED_Clear();
                while(Find_End() || abs(yaw-(last_yaw+180))>ANGLEError)
                {
                    while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0);
                    yaw+=180;   
                    scan();
                    Show_HW();
                    run();
                    MyOLED_ShowNum(1,1,
                        DL_TimerG_getCaptureCompareValue(PWM_1_INST, DL_TIMER_CC_0_INDEX),3);
                    MyOLED_ShowNum(2,1,
                        DL_TimerG_getCaptureCompareValue(PWM_1_INST, DL_TIMER_CC_1_INDEX),3); 
                }
                motorPidSetSpeed(0,0);
                DL_TimerG_startCounter(TIMER_0_INST);
                LED_ON();
                
            break;

            case 3:
                //斜线
                OLED_Clear();
                while(Find_Start())
                {
                    while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0);
                    double pid_output = calculate_pid(yaw,ANGLEOblique);
                    pwm1out = MID+pid_output;
                    pwm2out = MID-pid_output;
                
                    if(pwm1out >500) pwm1out=500;//进行限幅
                    if(pwm1out <0) pwm1out =0;
                    if(pwm2out >500) pwm2out =500;
                    if(pwm2out<0) pwm2out =0;
                    motorPidSetSpeed(pwm1out,pwm2out);//给参数电机参数的
                    
                    MyOLED_ShowNum(1,1,pwm1out,3);
                    MyOLED_ShowNum(2,1,pwm2out,3);
                    if(yaw>=0)
                    {
                        MyOLED_ShowNum(3,1,yaw,3);
                        MyOLED_ShowChar(3, 5, '+');
                    }
                    else {
                        MyOLED_ShowNum(3,1,-yaw,3);
                        MyOLED_ShowChar(3, 5, '-');
                    }
                }
                DL_TimerG_startCounter(TIMER_0_INST);
                LED_ON();

                //曲线
                OLED_Clear();
                while(Find_End() || abs(yaw-ANGLEStraight)>ANGLEError)
                {
                    while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0);
                    if(yaw<0)   yaw+=360;   
                    scan();
                    Show_HW();
                    run();
                    MyOLED_ShowNum(1,1,
                        DL_TimerG_getCaptureCompareValue(PWM_1_INST, DL_TIMER_CC_0_INDEX),3);
                    MyOLED_ShowNum(2,1,
                        DL_TimerG_getCaptureCompareValue(PWM_1_INST, DL_TIMER_CC_1_INDEX),3); 
                     MyOLED_ShowNum(4,1,yaw,3);   
                }
                DL_TimerG_startCounter(TIMER_0_INST);
                LED_ON();

                //斜线
                OLED_Clear();
                while(Find_Start())
                {
                    while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0);
                    if(yaw<0)   yaw += 360;
                    double pid_output = calculate_pid(yaw,ANGLEOblique2);
                    
                    pwm1out = MID+pid_output;
                    pwm2out = MID-pid_output;
                
                    if(pwm1out >500) pwm1out=500;//进行限幅
                    if(pwm1out <0) pwm1out =0;
                    if(pwm2out >500) pwm2out =500;
                    if(pwm2out<0) pwm2out =0;
                    motorPidSetSpeed(pwm1out,pwm2out);//给参数电机参数的
                    
                    MyOLED_ShowNum(1,1,pwm1out,3);
                    MyOLED_ShowNum(2,1,pwm2out,3);
                    if(yaw>=0)
                    {
                        MyOLED_ShowNum(3,1,yaw,3);
                        MyOLED_ShowChar(3, 5, '+');
                    }
                    else {
                        MyOLED_ShowNum(3,1,-yaw,3);
                        MyOLED_ShowChar(3, 5, '-');
                    }
                }
                DL_TimerG_startCounter(TIMER_0_INST);
                LED_ON();

                //曲线
                OLED_Clear();
                while(Find_End() || abs(yaw-(last_yaw+180))>ANGLEError)
                {
                    while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0);
                    yaw+=180;   
                    scan();
                    Show_HW();
                    run();
                    MyOLED_ShowNum(1,1,
                        DL_TimerG_getCaptureCompareValue(PWM_1_INST, DL_TIMER_CC_0_INDEX),3);
                    MyOLED_ShowNum(2,1,
                        DL_TimerG_getCaptureCompareValue(PWM_1_INST, DL_TIMER_CC_1_INDEX),3); 
                    MyOLED_ShowNum(4, 1, -(yaw-180), 3);
                }
                motorPidSetSpeed(0,0);
                DL_TimerG_startCounter(TIMER_0_INST);
                LED_ON();
            break;

            case 4:
                flag = 0;
                
                for(int i=0;i<4;i++)
                {
                    if(flag == 1)
                    {
                        flag = 0;
                        OLED_Clear();
                        do {
                            while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0);
                            double pid_output = calculate_pid(yaw,-17);
                            pwm1out = 100+pid_output;
                            pwm2out = 100-pid_output;
                            if(pwm1out >200) pwm1out=200;//进行限幅
                            else if(pwm1out <0) pwm1out =0;
                            if(pwm2out >200) pwm2out =200;
                            else if(pwm2out<0) pwm2out =0;
                            motorPidSetSpeed(pwm1out,pwm2out);//给参数电机参数的
                        
                            MyOLED_ShowNum(1,1,pwm1out,3);
                            MyOLED_ShowNum(2,1,pwm2out,3);
                            MyOLED_ShowNum(3,1,-yaw,3);
                        }while ((int)yaw!=-17);
                        motorPidSetSpeed(0,0);
                        while( mpu_dmp_init() );
                        do {
                            while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0);
                            MyOLED_ShowNum(4, 1, -yaw, 3);
                        }while ((int)yaw != -7);
                    }
                    //斜线
                    OLED_Clear();
                    while(Find_Start())
                    {
                        while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0);
                        double pid_output = calculate_pid(yaw,ANGLEOblique);
                        pwm1out = MID+pid_output;
                        pwm2out = MID-pid_output;
                    
                        if(pwm1out >500) pwm1out=500;//进行限幅
                        if(pwm1out <0) pwm1out =0;
                        if(pwm2out >500) pwm2out =500;
                        if(pwm2out<0) pwm2out =0;
                        motorPidSetSpeed(pwm1out,pwm2out);//给参数电机参数的
                        
                        MyOLED_ShowNum(1,1,pwm1out,3);
                        MyOLED_ShowNum(2,1,pwm2out,3);
                        if(yaw>=0)
                        {
                            MyOLED_ShowNum(3,1,yaw,3);
                            MyOLED_ShowChar(3, 5, '+');
                        }
                        else {
                            MyOLED_ShowNum(3,1,-yaw,3);
                            MyOLED_ShowChar(3, 5, '-');
                        }
                    }
                    DL_TimerG_startCounter(TIMER_0_INST);
                    LED_ON();

                    //曲线
                    OLED_Clear();
                    while(Find_End() || abs(yaw-ANGLEStraight)>ANGLEError)
                    {
                        while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0);
                        if(yaw<0)   yaw+=360;   
                        scan();
                        Show_HW();
                        run();
                        MyOLED_ShowNum(1,1,
                            DL_TimerG_getCaptureCompareValue(PWM_1_INST, DL_TIMER_CC_0_INDEX),3);
                        MyOLED_ShowNum(2,1,
                            DL_TimerG_getCaptureCompareValue(PWM_1_INST, DL_TIMER_CC_1_INDEX),3); 
                        MyOLED_ShowNum(4,1,yaw,3);   
                    }
                    DL_TimerG_startCounter(TIMER_0_INST);
                    LED_ON();

                    //斜线
                    OLED_Clear();
                    while(Find_Start())
                    {
                        while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0);
                        if(yaw<0)   yaw += 360;
                        double pid_output = calculate_pid(yaw,ANGLEOblique2);
                        
                        pwm1out = MID+pid_output;
                        pwm2out = MID-pid_output;
                    
                        if(pwm1out >500) pwm1out=500;//进行限幅
                        if(pwm1out <0) pwm1out =0;
                        if(pwm2out >500) pwm2out =500;
                        if(pwm2out<0) pwm2out =0;
                        motorPidSetSpeed(pwm1out,pwm2out);//给参数电机参数的
                        
                        MyOLED_ShowNum(1,1,pwm1out,3);
                        MyOLED_ShowNum(2,1,pwm2out,3);
                        if(yaw>=0)
                        {
                            MyOLED_ShowNum(3,1,yaw,3);
                            MyOLED_ShowChar(3, 5, '+');
                        }
                        else {
                            MyOLED_ShowNum(3,1,-yaw,3);
                            MyOLED_ShowChar(3, 5, '-');
                        }
                    }
                    DL_TimerG_startCounter(TIMER_0_INST);
                    LED_ON();

                    //曲线
                    OLED_Clear();
                    while(Find_End() || abs(yaw-(-17+180))>10)
                    {
                        while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0);
                        yaw+=180;   
                        scan();
                        Show_HW();
                        run();
                        MyOLED_ShowNum(1,1,
                            DL_TimerG_getCaptureCompareValue(PWM_1_INST, DL_TIMER_CC_0_INDEX),3);
                        MyOLED_ShowNum(2,1,
                            DL_TimerG_getCaptureCompareValue(PWM_1_INST, DL_TIMER_CC_1_INDEX),3); 
                    }
                    motorPidSetSpeed(0,0);
                    DL_TimerG_startCounter(TIMER_0_INST);
                    LED_ON();
                    flag = 1;   
                }
            break;
        }  
        MyOLED_ShowString(1, 1, "END");
    } 
}


int abs(int a)
{
    if(a>=0)    return a;
    else        return -a;
}

void Init_MPU_DMP()
{
    OLED_Clear();
    while(Key_GetChoice()!=1);
        while( mpu_dmp_init() )
        {
            printf("dmp error\r\n");
            delay_ms(200);
        }
        printf("Initialization Data Succeed \r\n");
        MyOLED_ShowString(1, 1, "OK");

        while(Key_GetChoice()!=1)
        {
            while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0);
            if(yaw>=0)
            {
                MyOLED_ShowNum(3,4,yaw,3);
                MyOLED_ShowChar(3, 1, '+');
            }
            else {
                MyOLED_ShowNum(3,4,-yaw,3);
                MyOLED_ShowChar(3, 1, '-');
            }
        }
        last_yaw = yaw;   
}

void MyOLED_ShowMode()
{
    OLED_Clear();
    MyOLED_ShowString(1, 1, (uint8_t*)"mode1");
    MyOLED_ShowString(2, 1, (uint8_t*)"mode2");
    MyOLED_ShowString(3, 1, (uint8_t*)"mode3");
    MyOLED_ShowString(4, 1, (uint8_t*)"mode4");
    MyOLED_ShowString(1, 7, (uint8_t*)"<");
}

void TIMER_0_INST_IRQHandler(void)
{
    switch (DL_TimerG_getPendingInterrupt(TIMER_0_INST)) {
        case DL_TIMER_IIDX_ZERO:
            LED_OFF();
            break;
        default:
            break;
    }
}
